﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using System.Drawing;
using HMMatrix;

namespace LocalizationTest
{
  class MBLTools
  {
    public static double CalculateFunction(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, double x, double y, Experiment experiment)
    {
      double sumOfDistances = 0;
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement worldStateAndObservation in listOfOldPlaces)
      {
        if (worldStateAndObservation.measurement != null)
        {
          WorldState2D worldState = new WorldState2D(x, y);
          worldState.Move((WorldState2D)worldStateAndObservation.worldState);
          Measurement prediction = worldStateAndObservation.measurement.sensor.PredictPerfect(worldState, experiment.map);
          double predictedValue = prediction.GetDifference(worldStateAndObservation.measurement);
          sumOfDistances += Math.Pow(predictedValue, 2);
        }//if (worldStateAndObservation.measurement != null)
      }// for each entry in history
      return sumOfDistances;
    }
    public static double CalculateFunction2(
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, 
      WorldState worldStateOrig, 
      Experiment experiment)
    {
      bool highWeightForNewerMeasurements = Globals.selfLocatorProperties.TypeOfLandmarkWeighting != WeightingType.allEqual;
      double sumOfDistances = 0;
      double weight;
      double index = 0;
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement worldStateAndObservation in listOfOldPlaces)
      {
        if (worldStateAndObservation.measurement != null)
        {
          double value = Math.Max(5 - index, 0);
          index++;
          //(listOfOldPlaces.Count - index) / listOfOldPlaces.Count;
          weight = Math.Pow(value, 2);
          WorldState worldState = (WorldState)worldStateOrig.Clone();
          worldState.Move(worldStateAndObservation.worldState);
//          Measurement prediction = worldStateAndObservation.measurement.sensor.PredictPerfect(worldState, experiment.map);
//          double difference = prediction.GetDifference(worldStateAndObservation.measurement);
          double difference = worldStateAndObservation.measurement.sensor.GetMeasurementDeviation(worldState, experiment.map, worldStateAndObservation.measurement);
          if(!highWeightForNewerMeasurements) weight = 1;
          sumOfDistances += Math.Pow(difference, 2) * weight;
        }//if (worldStateAndObservation.measurement != null)
      }// for each entry in history
      return sumOfDistances;
    }

    /*
    public static double CalculateFunction3(
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces,
      Vector vectorOrig,
      Experiment experiment)
    {
      bool highWeightForNewerMeasurements = Globals.selfLocatorProperties.TypeOfLandmarkWeighting != WeightingType.allEqual;
      double sumOfDistances = 0;
      double weight;
      double index = 0;
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement worldStateAndObservation in listOfOldPlaces)
      {
        if (worldStateAndObservation.measurement != null)
        {
          double value = Math.Max(5 - index, 0);
          index++;
          //(listOfOldPlaces.Count - index) / listOfOldPlaces.Count;
          weight = Math.Pow(value, 2);
          WorldState worldState = (WorldState)vectorOrig.Clone();
          worldState.Move(worldStateAndObservation.worldState);
          Measurement prediction = worldStateAndObservation.measurement.sensor.PredictPerfcet(worldState, experiment.map);
          double predictedValue = prediction.GetDifference(worldStateAndObservation.measurement);
          if (!highWeightForNewerMeasurements) weight = 1;
          sumOfDistances += Math.Pow(predictedValue, 2) * weight;
        }//if (worldStateAndObservation.measurement != null)
      }// for each entry in history
      return sumOfDistances;
    }
    */
      public static double evaluateLM(
        WorldState x,
        ActionAndObservationHistory.RelativePlaceWithMeasurement[] worldStateAndObservations,
        Experiment experiment)
      {
        double res = frobeniusNorm(df(x,worldStateAndObservations,experiment));
        return res;
      }//end evaluateLM

      public static double[,] df(
        WorldState x,
        ActionAndObservationHistory.RelativePlaceWithMeasurement[] worldStateAndObservations,
        Experiment experiment)
      {
          int n = worldStateAndObservations.Length;
          double[,] der = dg(x, worldStateAndObservations[0], experiment);
          int m = der.GetLength(1);

          double[,] dev = new double[n,m];

          for (int i = 0; i < n; i++)
          {
              der = dg(x, worldStateAndObservations[i], experiment);
              for (int j = 0; j < m; j++)
              {
                  dev[i, j] = der[0, j];
              }//end for
          }//end for


          double[,] M = Matrix.Multiply(Matrix.Transpose(dev), dev);
          if (n == 1)
              M[0, 0] = 1 / M[0, 0];
          else
          {
              double[,] Mt = new double[m + 1, m + 1];
              for (int i = 0; i < m; i++)
                  for (int j = 0; j < m; j++)
              {
                  Mt[i + 1, j + 1] = M[i, j];
              }
              Matrix.inverse(ref Mt, m);
              for (int i = 0; i < m; i++)
                  for (int j = 0; j < m; j++)
                  {
                      M[i, j] = Mt[i + 1, j + 1];
                  }
          }
         

          double[,] res = Matrix.Multiply(M, Matrix.Transpose(dev));
          
          return res;
      }//end df

      public static double frobeniusNorm(double[,] M)
      {
          double res = 0;
          int rows = M.GetLength(0);
          int cols = M.GetLength(1);

          for (int i = 0; i < rows; i++)
          {
              for (int j = 0; j < cols; j++)
              {
                  res += M[i, j] * M[i, j];
              }//end for
          }//end for

          return Math.Sqrt(res);
      }//end frobeniusNorm

      // worldStateAndObservation defines the function g
      public static double g(
          WorldState x, 
          ActionAndObservationHistory.RelativePlaceWithMeasurement worldStateAndObservation,
          Experiment experiment)
      {
          WorldState worldState = (WorldState)x.Clone();
          worldState.Move(worldStateAndObservation.worldState);
          Measurement prediction = worldStateAndObservation.measurement.sensor.PredictPerfect(worldState, experiment.map);
          return prediction.GetValueAsDouble();
      }//end g

     public static double[,] dg(
        WorldState worldState,
        ActionAndObservationHistory.RelativePlaceWithMeasurement worldStateAndObservation,
        Experiment experiment)
    {
        double epsilon = 1e-8;
        double[,] res = null;

        if (worldState is WorldStatePose2D)
        {
            res = new double[1,3];
            double x = (worldState as WorldStatePose2D).Pose.translation.x;
            double y = (worldState as WorldStatePose2D).Pose.translation.y;
            double a = (worldState as WorldStatePose2D).Pose.rotation;

            res[0, 0] = (g(new WorldStatePose2D(x + epsilon, y, a), worldStateAndObservation, experiment) - g(new WorldStatePose2D(x - epsilon, y, a), worldStateAndObservation, experiment)) / (2 * epsilon);
            res[0, 1] = (g(new WorldStatePose2D(x, y + epsilon, a), worldStateAndObservation, experiment) - g(new WorldStatePose2D(x, y - epsilon, a), worldStateAndObservation, experiment)) / (2 * epsilon);
            res[0, 2] = (g(new WorldStatePose2D(x, y, a + epsilon), worldStateAndObservation, experiment) - g(new WorldStatePose2D(x, y, a - epsilon), worldStateAndObservation, experiment)) / (2 * epsilon);
        }

        if (worldState is WorldState2D)
        {
            res = new double[1, 2];
            double x = (worldState as WorldState2D).Vector.x;
            double y = (worldState as WorldState2D).Vector.y;

            res[0, 0] = (g(new WorldState2D(x + epsilon, y), worldStateAndObservation, experiment) - g(new WorldState2D(x - epsilon, y), worldStateAndObservation, experiment)) / (2 * epsilon);
            res[0, 1] = (g(new WorldState2D(x, y + epsilon), worldStateAndObservation, experiment) - g(new WorldState2D(x, y - epsilon), worldStateAndObservation, experiment)) / (2 * epsilon);
        }

        if (worldState is WorldState1D)
        {
            res = new double[1, 1];
            double x = (worldState as WorldState1D).Position;

            res[0, 0] = (g(new WorldState1D(x + epsilon), worldStateAndObservation, experiment) - g(new WorldState1D(x - epsilon), worldStateAndObservation, experiment)) / (2 * epsilon);
        }

        return res;
    }//end g

    public static Vector2 CalculateGradient(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, double x, double y, Experiment setup)
    {
      double lowX = MBLTools.CalculateFunction(listOfOldPlaces, x - 1, y, setup);
      double highX = MBLTools.CalculateFunction(listOfOldPlaces, x + 1, y, setup);
      double lowY = MBLTools.CalculateFunction(listOfOldPlaces, x, y - 1, setup);
      double highY = MBLTools.CalculateFunction(listOfOldPlaces, x, y + 1, setup);

      return new Vector2(highX - lowX, highY - lowY);
    }

    public static Vector2 CalculateSumOfGradientsForFunction(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, double x, double y, Experiment experiment)
    {
      Vector2 sumOfGradients = new Vector2(0, 0);
      Vector2 lookInsidePosition = new Vector2(100, 100);
      foreach (ActionAndObservationHistory.RelativePlaceWithMeasurement worldStateAndObservation in listOfOldPlaces)
      {
        if (worldStateAndObservation.measurement != null)
        {
          WorldState2D worldState = new WorldState2D(x, y);
          worldState.Move((WorldState2D)worldStateAndObservation.worldState);
          if (x == lookInsidePosition.x && y == lookInsidePosition.y)
          {
            DebugDrawingTool.CreateCircle("world:look inside", new Vector2(worldState.Vector.x, worldState.Vector.y), 2, 2, Color.Brown);
          }
          Measurement prediction = worldStateAndObservation.measurement.sensor.PredictPerfect(worldState, experiment.map);
          double predictionMeasurementDifference = prediction.GetDifference(worldStateAndObservation.measurement);

          //gradient calculation
          WorldState2D worldStateLowX = new WorldState2D(worldState.Vector.x - 1, worldState.Vector.y);
          WorldState2D worldStateHighX = new WorldState2D(worldState.Vector.x + 1, worldState.Vector.y);
          WorldState2D worldStateLowY = new WorldState2D(worldState.Vector.x, worldState.Vector.y - 1);
          WorldState2D worldStateHighY = new WorldState2D(worldState.Vector.x, worldState.Vector.y + 1);
          double lowX = worldStateAndObservation.measurement.sensor.PredictPerfect(worldStateLowX, experiment.map).GetValueAsDouble();
          double highX = worldStateAndObservation.measurement.sensor.PredictPerfect(worldStateHighX, experiment.map).GetValueAsDouble();
          double lowY = worldStateAndObservation.measurement.sensor.PredictPerfect(worldStateLowY, experiment.map).GetValueAsDouble();
          double highY = worldStateAndObservation.measurement.sensor.PredictPerfect(worldStateHighY, experiment.map).GetValueAsDouble();

          Vector2 gradient = new Vector2(highX - lowX, highY - lowY);
          if (gradient.Abs() != 0) gradient /= gradient.Abs();
          gradient *= predictionMeasurementDifference;
          if (x == lookInsidePosition.x && y == lookInsidePosition.y)
          {
            Vector2 current = new Vector2(worldState.Vector.x, worldState.Vector.y);
            DebugDrawingTool.CreateLine("world:look inside", current, current + gradient, 3, Color.Red);
            Vector2 start = new Vector2(x, y);
            DebugDrawingTool.CreateLine("world:look inside", start, start - gradient, 3, Color.Green);
            DebugDrawingTool.CreateCircle("world:look inside", new Vector2(worldState.Vector.x, worldState.Vector.y), prediction.GetValueAsDouble(), 3, Color.Pink);
            DebugDrawingTool.CreateCircle("world:look inside", new Vector2(worldState.Vector.x, worldState.Vector.y), 3, 2, Color.Red);
          }
          sumOfGradients += gradient;
        }//if (worldStateAndObservation.measurement != null)
      }// for each entry in history
      return sumOfGradients;
    }
    public static double CalculateLaplace(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, double x, double y, Experiment setup)
    {
      int[,] matrix = new int[3, 3] { { 1, 1, 1 }, { 1, -8, 1 }, { 1, 1, 1 } };
      double laplace = 0;
      for (int xOffset = -1; xOffset <= 1; xOffset++)
      {
        for (int yOffset = -1; yOffset <= 1; yOffset++)
        {
          laplace += (1 / 3.0 * matrix[xOffset + 1, yOffset + 1] * MBLTools.CalculateFunction(listOfOldPlaces, x + xOffset, y + yOffset, setup));
        }
      }

      return laplace;
    }

    public static WorldState2D DoGradientDescent(Vector2 startPosition, Experiment experiment)
    {
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);

      int numberOfIterations = 60;

      for (int i = 0; i < numberOfIterations; i++)
      {
        Vector2 oldPosition = startPosition;
        Vector2 gradient = MBLTools.CalculateGradient(listOfOldPlaces, startPosition.x, startPosition.y, experiment);
        gradient /= gradient.Abs();
        startPosition -= (gradient * 10.0);
        int intensity = (int)((numberOfIterations - i) * 200.0 / numberOfIterations);
        Color color = Color.FromArgb(200 - intensity, 0, intensity);
        DebugDrawingTool.CreateLine("world:MBL:gradient descent path", oldPosition, startPosition, 4, color);
      }
      return new WorldState2D(startPosition.x, startPosition.y);
    }


    public static WorldState2D DoIntelligentGradientDescent(Vector2 startPosition, Experiment experiment)
    {
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);

      int numberOfIterations = 250;

      for (int i = 0; i < numberOfIterations; i++)
      {
        Vector2 oldPosition = startPosition;
        Vector2 gradient = MBLTools.CalculateSumOfGradientsForFunction(listOfOldPlaces, startPosition.x, startPosition.y, experiment);
        gradient /= gradient.Abs();
        startPosition -= (gradient * 10.0);
        int intensity = (int)((numberOfIterations - i) * 200.0 / numberOfIterations);
        Color color = Color.FromArgb(0, 200 - intensity, intensity);
        DebugDrawingTool.CreateLine("world:MBL:intelligent gradient descent path", oldPosition, startPosition, 4, color);
      }
      return new WorldState2D(startPosition.x, startPosition.y);
    }

    /*
    public void CalculateLaplaceForAll(Vector2 bottomLeftCorner, Vector2 topRightCorner)
    {
      if (actionAndObservationHistory.history.Count == 0) return;
      List<ActionAndObservationHistory.RelativePlacesInfo> listOfOldPlaces = actionAndObservationHistory.GetRelativePlacesNew(simulatedWorldState);
      if (listOfOldPlaces.Count == 0) return;

      List<Function2D> function = new List<Function2D>();
      int rasterWidth = Globals.rasterWidth;
      int xCounter = 0;
      int yCounter = 0;

      double maximalValue = 0;
      double minimalValue = Double.MaxValue;

      for (int x = (int)bottomLeftCorner.x; x <= (int)topRightCorner.x; x += rasterWidth)
      {
        yCounter = 0;
        for (int y = (int)bottomLeftCorner.y; y <= (int)topRightCorner.y; y += rasterWidth)
        {
          double value = -CalculateLaplace(listOfOldPlaces, x, y);
          function.Add(new Function2D(x, y, value));

          if (value > maximalValue)
            maximalValue = value;
          if (value < minimalValue)
            minimalValue = value;

          yCounter++;
        }//for y..
        xCounter++;
      }//for x..
      DrawFunction("laplace", function, minimalValue, maximalValue, rasterWidth, false, bottomLeftCorner, topRightCorner);
    }
    */
  }
}
